#include "GMSimPhys.hh"


/**
 * Inicia a física do simulador.
 */
GMSimPhys::GMSimPhys()
{
	/* Seta posições do mundo */
	FIELD_BL = cpv(100.0, 50.0);
	FIELD_BR = cpv(1200.0, 50.0);
	FIELD_UR = cpv(1200.0, 750.0);
	FIELD_UL = cpv(100.0, 750.0);
	WORLD_BL = cpv(50.0, 0.0);
	WORLD_BR = cpv(1250.0, 0.0);
	WORLD_UR = cpv(1250.0, 800.0);
	WORLD_UL = cpv(50.0, 800.0);
	GOALOUR_B = cpv(100.0, 330.0);
	GOALOUR_U = cpv(100.0, 450.0);
	GOALOPP_B = cpv(1200.0, 330.0);
	GOALOPP_U = cpv(1200.0, 450.0);
	placar_a = 0;
	placar_b = 0;

	/* Inicia o Chipmunk */
	cpInitChipmunk();

	/** Inicia as entidades físicas **/
	int i;

	/* inicia o espaço */
	space = cpSpaceNew();
	space->gravity = cpv(0.0f, 0.0f);
	
	/* inicia os robôs */
	float px[10] = { 120.0, 260.0, 380.0, 380.0, 500.0, 
		     	1180.0, 1040.0, 920.0, 920.0, 810.0 };
	float py[10] = { 400.0, 400.0, 575.0, 225.0, 400.0, 
		     	400.0, 400.0, 575.0, 225.0, 400.0 };
	for (i = 0; i < NUM_ROB; i++) {
		kick[i] = 0;
		dribble[i] = 0;

		robot[i] = cpBodyNew(ROBOT_MASS, ROBOT_MASS);
		robot[i]->p = cpv(px[i], py[i]);
		robot[i]->a = 0.0;
		cpSpaceAddBody(space, robot[i]);

		cpShape *bShape = cpCircleShapeNew(robot[i], ROBOT_RADIUS, cpv(0.0f, 0.0f));
		bShape->e = 0.4;
		bShape->u = 100.0;
		cpSpaceAddShape(space, bShape);
	}

	/* inicia a bola */
	ball = cpBodyNew(0.1f, 0.1f);
	ball->p = cpv(500.0f, 480.0f);
	ball->v = cpv(0.0f, 0.0f);

	cpShape *bShape = cpCircleShapeNew(ball, BALL_RADIUS, cpv(0.0f, 0.0f));
	bShape->e = 0.4;
	bShape->u = 100.0;
	cpSpaceAddBody(space, ball);
	cpSpaceAddShape(space, bShape);

	/* inicia as "laterais" do campo */
	cpShape *seg;
	cpBody *staticBody = cpBodyNew(INFINITY, INFINITY);
	seg = cpSegmentShapeNew(staticBody, FIELD_BL, FIELD_BR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_BR, GOALOPP_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_B, cpv(WORLD_BR.x, GOALOPP_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_B.y), cpv(WORLD_BR.x, GOALOPP_U.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_U.y), GOALOPP_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_U, FIELD_UR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UR, FIELD_UL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UL, GOALOUR_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_U, cpv(WORLD_BL.x, GOALOUR_U.y) , 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_U.y), cpv(WORLD_BL.x, GOALOUR_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_B.y), GOALOUR_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_B, FIELD_BL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(space, seg);
}

/**
 * Atualiza as entidades físicas.
 */
void 
GMSimPhys::update()
{
	/* quanto maior, mais precisos são os cálculos. */
	int steps = 1, i;
	static int placar_aux=1;
	cpFloat dt = 1.0/60.0/(cpFloat)steps;

	/* passa o angulo positivo em graus */
	unsigned int ang[NUM_ROB];
	for (i = 0; i < NUM_ROB; i++) {
		if (robot[i]->a >= 0) 
			ang[i] = robot[i]->a*RAD2DEG;
		else
			ang[i] = (robot[i]->a + 2*PI)*RAD2DEG;
	}
	
	/* Se a bola foi chutada */
	for (i = 0; i < NUM_ROB; ++i) {
		if (kick[i]) {
			if (dist(ball->p, cpv(robot[i]->p.x + cos(robot[i]->a)*ROBOT_RADIUS,
					robot[i]->p.y + sin(robot[i]->a)*ROBOT_RADIUS)) < 15.0)
				cpBodyApplyImpulse(ball, cpvmult(cpvsub(ball->p, robot[i]->p), kick[i]/2.0),cpvzero);
		}
		if (dribble[i]) {
			if (dist(ball->p, cpv(robot[i]->p.x + cos(robot[i]->a)*ROBOT_RADIUS,
					robot[i]->p.y + sin(robot[i]->a)*ROBOT_RADIUS)) < 15.0) {
				ball->p.x = robot[i]->p.x + cos(robot[i]->a)*ROBOT_RADIUS;
				ball->p.y = robot[i]->p.y + sin(robot[i]->a)*ROBOT_RADIUS;
			}
			else {
				cpBodyResetForces(ball);
			}
		}

		kick[i] = 0;
	}

	/* conta os gols */
	if (ball->p.x < FIELD_BL.x && placar_aux) {
		placar_a++;
		placar_aux = 0;
	}	
	else if (ball->p.x > FIELD_BR.x && placar_aux) {
		placar_b++;
		placar_aux = 0;
	}
	else if (ball->p.x < FIELD_BR.x && ball->p.x > FIELD_BL.x) {
		placar_aux = 1;
	}

	for (i = 0; i < NUM_ROB; i++) {
		robot[i]->v.x = (rob_vx[i]*cos(robot[i]->a) - rob_vy[i]*sin(robot[i]->a)) * VEL2MPS;
		robot[i]->v.y = (rob_vx[i]*sin(robot[i]->a) + rob_vy[i]*cos(robot[i]->a)) * VEL2MPS;
	}

	/* atualiza o Chipmunk */
	cpSpaceStep(space, dt);

}

/** Pega a posição da bola.
 *
 * @param *ballpos Receberá a posição da bola.
 */
void 
GMSimPhys::get_ballpos( float * ballpos )
{
	ballpos[0] = ball->p.x;
	ballpos[1] = ball->p.y;
}

/** Seta a posição da bola.
 *
 * @param *ballpos Nova posição da bola.
 */
void 
GMSimPhys::set_ballpos( float * ballpos )
{
	ball->p.x = ballpos[0];
	ball->p.y = ballpos[1];
	ball->v = cpvzero;
}

/** Seta a velocidade da bola.
 *
 * @param *ballvel Nova velocidade da bola.
 */
void
GMSimPhys::set_ballvel( float * ballvel )
{
	ball->v.x = ballvel[0];
	ball->v.y = ballvel[1];
}

/** Pega a posição de um robô.
 *
 * @param *robotpos Receberá a posição do robô.
 *
 * @param i Identificador do robô.
 */
void 
GMSimPhys::get_robotpos( float * robotpos, int i )
{
	robotpos[0] = robot[i]->p.x;
	robotpos[1] = robot[i]->p.y;
}

/** Seta a posição de um robô.
 *
 * @param *robotpos Nova posição do robô.
 *
 * @param i Identificador do robô.
 */
void 
GMSimPhys::set_robotpos( float * robotpos, int i )
{
	robot[i]->p.x = robotpos[0];
	robot[i]->p.y = robotpos[1];
	robot[i]->v = cpvzero;
	robot[i]->w = 0.0;
}

/** Seta velocidade de um robô.
 *
 * @param *robotvel Nova velocidade.
 *
 * @param w Nova velocidade angular.
 *
 * @param i Identificador do robô.
 */
void
GMSimPhys::set_robotvel( float * robotvel, float w, int i )
{
	rob_vx[i] = robotvel[0]; rob_vy[i] = robotvel[1];
	robot[i]->v.x = (robotvel[0]*cos(robot[i]->a) - robotvel[1]*sin(robot[i]->a)) * VEL2MPS;
	robot[i]->v.y = (robotvel[0]*sin(robot[i]->a) + robotvel[1]*cos(robot[i]->a)) * VEL2MPS;
	robot[i]->w = w;
}

/** Pega o ângulo de um robô.
 *
 * @param *robotang Ângulo do robô.
 *
 * @param i Identificador do robô.
 */
void 
GMSimPhys::get_robotang( float * robotang, int i )
{
	*robotang = robot[i]->a;
}

/** Pega o mundo.
 *
 * @param *field Posições do campo.
 */
void
GMSimPhys::get_world( float * world )
{
	world[0] = WORLD_BL.x;
	world[1] = WORLD_BL.y;
	world[2] = WORLD_BR.x;
	world[3] = WORLD_BR.y;
	world[4] = WORLD_UR.x;
	world[5] = WORLD_UR.y;
	world[6] = WORLD_UL.x;
	world[7] = WORLD_UL.y;
	world[8] = FIELD_BL.x;
        world[9] = FIELD_BL.y;
        world[10] = FIELD_BR.x;
        world[11] = FIELD_BR.y;
        world[12] = FIELD_UR.x;
	world[13] = FIELD_UR.y;
	world[14] = FIELD_UL.x;
	world[15] = FIELD_UL.y;
	world[16] = (FIELD_UR.x + FIELD_UL.x)/2.0;
	world[17] = FIELD_UL.y;
	world[18] = world[16];
	world[19] = FIELD_BL.y;
	world[20] = GOALOUR_U.x;
	world[21] = GOALOUR_U.y;
	world[22] = GOALOUR_B.x;
	world[23] = GOALOUR_B.y;
	world[24] = GOALOPP_U.x;
	world[25] = GOALOPP_U.y;
	world[26] = GOALOPP_B.x;
	world[27] = GOALOPP_B.y;
}

/** Faz um robô chutar.
 *
 * @param level Nível (força) do chute.
 *
 * @param i Identificador do robô.
 */
void
GMSimPhys::robot_kick( int level, 
		       int i )
{
	kick[i] = level;
}

/** Liga/Desliga o rolinho de um robô.
 *
 * @param enable Se for 0 desativa o rolinho, caso contrário ativa o rolinho.
 *
 * @param i Identificador do robô.
 */
void
GMSimPhys::robot_dribble( int enable,
			  int i )
{
	dribble[i] = enable;
}

float 
GMSimPhys::dist( cpVect a, 
		 cpVect b )
{
	return sqrt( pow(a.x - b.x, 2) + pow(a.y - b.y, 2) );
}
